/*
实现小车的运动控制
*/
#include <Arduino.h>
#include "GET_IMU.h" //@JY901()  @GET_Angle() @Print_Angel()
#include "FOC_Setting.h"
#include "CONTROL.h"



float TarSpd=0,SteerSpd=0,roboSpdSen;
float drvVoltage=0,ExPitch=0,IntgErrV;
//float Kpv=1.997,Kiv=0.004,Kpa=0.196,Kda=0.03636,IntgMax=400;
float Kpv=0,Kiv=0,Kpa=0,Kda=0,IntgMax=0;

void Control()
{
    //该函数用于对小车的运动控制
    //小车采用传统的串级PID控制
    //《速度环》 根据当前速度与期望速度之差输出期望角度至《平衡环》⬇
    //《平衡环》 根据当前倾角与期望倾角之差输出《电机驱动力矩》使小车角度趋近期望角度
    static float ErrAve=0;
    static float ErrV[(SMOOTH_FILTER+1)];
    unsigned char i,j;

    roboSpdSen=(sensor.getVelocity()-sensor1.getVelocity())*0.5; //运动学正解计算当前车速
    for(i=0;i<SMOOTH_FILTER;i++){ErrV[i]=ErrV[i+1];}
    ErrV[SMOOTH_FILTER]=TarSpd-roboSpdSen;                  
    for(j=0;j<=SMOOTH_FILTER;j++) {ErrAve+=ErrV[j];}//对返回速度进行平滑滤波（求当前返回速度和前“SMOOTH_FILTER”个数据的平均值）消除数据抖动
    ErrAve=ErrAve/(SMOOTH_FILTER+1);

    IntgErrV+=ErrV[0];//对ErrAve积分                                
    if(IntgErrV<(-IntgMax)){IntgErrV=-IntgMax;}
    else if(IntgErrV>IntgMax){IntgErrV=IntgMax;}//积分限幅

    ExPitch=Kpv*ErrAve+Kiv*IntgErrV;//PID计算计算公式本体 速度环输出期望角度
    drvVoltage=Kpa*(ExPitch-JY901Pitch)+Kda*JY901Wy;//PID计算计算公式本体 平衡环输出电机力矩
}
void MotMove()
{
    //调用SimpleFOC库中的函数实现电机运动控制
    motor.loopFOC();
    motor1.loopFOC();
    motor.move(drvVoltage-SteerSpd*0.05); 
    motor1.move(drvVoltage+SteerSpd*0.05);
    //SteerSpd变量控制小车转向（小车方向没有用陀螺仪数据做闭环）
}
